#include "RobotDriveCustom.h"

namespace Team675
{
			
RobotDrive::RobotDrive(DriveType *_drive) : 
		m_rampArmRelay(RAMP_ARM),
		m_rampArmServo(RAMP_ARM_SERVO)
{
	m_drive = _drive;
	
	m_leftVictors = new DoubleVictor(LEFT_VICTOR_1, LEFT_VICTOR_2);
	m_rightVictors = new DoubleVictor(RIGHT_VICTOR_1, RIGHT_VICTOR_2);
//	m_leftEncoder = new Encoder(LEFT_ENCODER);
//	m_rightEncoder = new Encoder(RIGHT_ENCODER);
//	m_leftSolenoidOn = new Solenoid(LEFT_SOLENOID_ON);
//	m_leftSolenoidOff = new Solenoid(LEFT_SOLENOID_OFF);
//	m_rightSolenoidOn = new Solenoid(RIGHT_SOLENOID_ON);
//	m_rightSolenoidOff = new Solenoid(RIGHT_SOLENOID_OFF);
	m_leftSolenoidOn = m_rightSolenoidOn = new Solenoid(SOLENOID_ON);
	m_leftSolenoidOff = m_rightSolenoidOff = new Solenoid(SOLENOID_OFF);
	
	m_compressor = new Compressor(COMPRESSOR_SWITCH,COMPRESSOR_CHANNEL);
	m_compressor->Start();

//	m_leftEncoder->SetPIDSourceParameter(Encoder::kRate);
//	m_rightEncoder->SetPIDSourceParameter(Encoder::kRate);
//	m_leftEncoder->Reset();
//	m_rightEncoder->Reset();
//	m_leftEncoder->Start();
//	m_rightEncoder->Start();

	Shift(LOW_GEAR);
	
	m_info = *m_drive->Get();
	
	m_rampArmIsDown = false;
	m_rampArmTimer.Reset();
	m_rampArmTimer.Start();
	
	m_rampServoIsIn = false;
		
//	m_rightPID = new SendablePIDController(DRIVE_PID_PARAMS, m_rightEncoder, m_rightVictors);
//	m_rightPID->SetInputRange(-3000,3000);
//	m_rightPID->SetOutputRange(-1,1);
//	m_rightPID->Disable();
//	
//	m_leftPID = new SendablePIDController(DRIVE_PID_PARAMS, m_leftEncoder,  m_leftVictors);
//	m_leftPID->SetInputRange(-3000,3000);
//	m_leftPID->SetOutputRange(-1,1);
//	m_leftPID->Disable();
}

RobotDrive::~RobotDrive()
{
	delete m_rightPID;
	delete m_leftPID;
}

void RobotDrive::Update()
{
	DriveInfo t_newInfo = *m_drive->Get();
	
	m_shiftSetting = t_newInfo.Shift;
	
	if(t_newInfo.IsPID != m_info.IsPID)
	{
		if(t_newInfo.IsPID)
		{
			InitPID();
		}
		else
		{
			KillPID();
		}
	}
		
	if(t_newInfo.RampArmDown && !m_rampArmIsDown/* && m_rampArmTimer.HasPeriodPassed(RAMP_ARM_TIME)*/)
	{
		m_rampArmIsDown = true;
		m_rampArmTimer.Reset();
		m_rampArmTimer.Start();
	}
	else if(!t_newInfo.RampArmDown && m_rampArmIsDown/* && m_rampArmTimer.HasPeriodPassed(RAMP_ARM_TIME)*/)
	{
		m_rampArmIsDown = false;
		m_rampArmTimer.Reset();
		m_rampArmTimer.Start();
	}
	
	if(m_rampArmTimer.Get() < RAMP_ARM_TIME)
	{
		if(m_rampArmIsDown)
			m_rampArmRelay.Set(Relay::kForward);
		else
			m_rampArmRelay.Set(Relay::kReverse);
	}
	else m_rampArmRelay.Set(Relay::kOff);
	
	m_rampServoIsIn = t_newInfo.RampServoIn;
	
	if(m_rampServoIsIn) m_rampArmServo.SetAngle(RAMP_SERVO_IN);
	else m_rampArmServo.SetAngle(RAMP_SERVO_OUT);
	
	DriveUpdate(t_newInfo);
	ShiftUpdate(t_newInfo);
	
	m_info = t_newInfo;
}

void RobotDrive::SetDrive(DriveType * _drive)
{
	m_drive = _drive;
}

void RobotDrive::ShiftUpdate(DriveInfo &_info)
{
	if (_info.Shift == DriveType::ShiftHigh)
		Shift(HIGH_GEAR);
	else if (_info.Shift == DriveType::ShiftLow)
		Shift(LOW_GEAR);
//	else if (_info.Shift == DriveType::ShiftAuto)
//	{
//		if(m_shiftState)
//		{
//			if((-m_leftEncoder->GetRate() + m_rightEncoder->GetRate() / 2) > DRIVE_SHIFT_TRESHOLD_LOWER)
//				Shift(HIGH_GEAR);
//			else
//				Shift(LOW_GEAR);
//		}
//		else
//		{
//			if((-m_leftEncoder->GetRate() + m_rightEncoder->GetRate() / 2) > DRIVE_SHIFT_TRESHOLD_UPPER)
//				Shift(HIGH_GEAR);
//			else
//				Shift(LOW_GEAR);
//		}
//	}
}

void RobotDrive::DriveUpdate(DriveInfo &_info)
{
	if(_info.IsPID)
	{
		m_leftPID->SetSetpoint(_info.LeftSpeed * 3000);
		m_rightPID->SetSetpoint(-_info.RightSpeed * 3000);
		
		if(DEBUG_MODE)
		{
			SmartDashboard::GetInstance()->PutData("Left Drive PID", m_leftPID);
			SmartDashboard::GetInstance()->PutData("Right Drive PID", m_rightPID);
		}
	}
	else
	{
		m_leftVictors->Set(-_info.LeftSpeed);
		m_rightVictors->Set(_info.RightSpeed);
	}
	
	SmartDashboard::GetInstance()->PutDouble("Left Motor Speed", (double) _info.LeftSpeed);
	SmartDashboard::GetInstance()->PutDouble("Right Motor Speed", (double) _info.RightSpeed);
//	SmartDashboard::GetInstance()->PutDouble("Left Encoder Speed", (double) -m_leftEncoder->GetRate());
//	SmartDashboard::GetInstance()->PutDouble("Right Encoder Speed", (double) m_rightEncoder->GetRate());
}

bool RobotDrive::GetAutoShiftHigh()
{
	return false;
}

void RobotDrive::Shift(bool _high)
{
	m_leftSolenoidOn->Set(_high);
	m_leftSolenoidOff->Set(!_high);
	m_rightSolenoidOn->Set(_high);
	m_rightSolenoidOff->Set(!_high);
	
	if(_high == HIGH_GEAR)
	{
		SmartDashboard::GetInstance()->PutString("Gear Setting", "High");
	}
	else
	{
		SmartDashboard::GetInstance()->PutString("Gear Setting", "Low");
	}
}


void RobotDrive::InitPID()
{
	m_leftPID->Enable();
	m_rightPID->Enable();
}


void RobotDrive::KillPID()
{
	m_rightPID->Disable();
	m_leftPID->Disable();
}
	
}
